#include <cstdio>
#include <chrono>
#include <memory>
#include <iostream>
#include "main_circle.hpp"
#include "yaogan.hpp"
#include "rclcpp/rclcpp.hpp"
//#include "rcl_interfaces/msg/set_parameters_result.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "std_msgs/msg/int32_multi_array.hpp"
#include "std_msgs/msg/string.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
using namespace std::chrono_literals;
using std::placeholders::_1;

class Motor_Driver : public rclcpp::Node
{
  public:
    Motor_Driver() : Node("motor_driver")
    {
      // declare_parameter<int>("motor_max_rpm");  
      // declare_parameter<double>("lidar_range");
      // 声明参数并设置默认值
        this->declare_parameter<int>("yaml_run_mode",0);
        this->declare_parameter<int>("yaml_angle_n",0);
        this->declare_parameter<int>("yaml_angle_set",0);
        yaml_run_mode =this->get_parameter("yaml_run_mode").as_int();
        yaml_angle_n = this->get_parameter("yaml_angle_n").as_int();
        yaml_angle_set = this->get_parameter("yaml_angle_set").as_int();
        //double range =this->get_parameter("lidar_range").as_double();  
        RCLCPP_INFO(this->get_logger(),"yaml_run_mode=%d yaml_angle_n=%d yaml_angle_set=%d",yaml_run_mode,yaml_angle_n,yaml_angle_set);

        odom_publisher_ = this->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
        motor_info_ = this->create_publisher<std_msgs::msg::Int32MultiArray>("motor_info", 10);
        
        //订阅topic，接收目标点坐标，暂定名为topic
        topic_sub_ = this->create_subscription<std_msgs::msg::String>(
            "topic", 10, std::bind(&Motor_Driver::topic_gettarget_callback, this, _1));

        timer_ = this->create_wall_timer(20ms, std::bind(&Motor_Driver::timer_callback, this));
        motor_control_ = this->create_subscription<std_msgs::msg::Int32MultiArray>(
          "motor_control", 10, std::bind(&Motor_Driver::topic_callback, this, _1));

        // 初始化 tf2 缓冲区和监听器
        tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, this);

        mc = new Motor_Circle("ttyS4",1000000);
        yg=new yaogan("ttyS6",115200);
        last_time_ = this->get_clock()->now();
    }
    void timer_callback()
    {
      int speed,zhuan_sp;
        
        if(yaml_run_mode==1)
        {
        speed=yg->yg_value.Left_horiz*20;
        zhuan_sp=yg->yg_value.Right_horiz*10;
        std::cout<<"yaogan="<<speed<<"current= "<<mc->speed_cur[yaml_angle_n]<<std::endl;
        std::cout<<"zhuan_sp="<<zhuan_sp<<"current= "<<mc->zhuan_cur[yaml_angle_n]<<std::endl;
        mc->speed_set[yaml_angle_n]=speed;
        mc->zhuan_set[yaml_angle_n]=zhuan_sp;
        }
        if(yaml_run_mode==2)
        {
         mc->mip->angle_set[yaml_angle_n]=yaml_angle_set;
         mc->swap_info();
         mc->mip->run_set_angle();
         mc->swap_ctr();
        }
        if(yaml_run_mode==0)
        {
          linex=mc->mip->convert_yaogan_line(yg->yg_value.Left_vertic);
          liney=mc->mip->convert_yaogan_line(yg->yg_value.Left_horiz/2);
          wmga =(float)(mc->mip->convert_yaogan_wmga(yg->yg_value.Right_horiz));
          mc->set_motor_line(linex,liney,wmga);
          mc->speed_set[1]=mc->mip->convert_ms_to_rmp(linex);
       //  std::cout<<"lx="<<linex<<" ly="<<liney<<" wmga="<<wmga<<std::endl;
       publish_odometry();
       mc->timer_callback();
        }
        get_current_pose();
        //  publish_odometry();
        // mc->timer_callback();
       
    }
    void topic_callback(const std_msgs::msg::Int32MultiArray::SharedPtr msg) const
    {
        
    }

    void topic_gettarget_callback(const std_msgs::msg::String::SharedPtr msg)
    {
        RCLCPP_INFO(this->get_logger(), "Received from node: '%s'", msg->data.c_str());
    }

    void publish_odometry()
    {
        auto message = nav_msgs::msg::Odometry();
        // 假设这些是通过某种方式计算得到的速度
        double vx = linex;  // 线速度x
        double vy = liney;  // 线速度y
        double w = wmga;   // 角速度

        // 更新位置和姿态
        current_time_ = this->get_clock()->now();
        double dt = (current_time_ - last_time_).seconds();

        // 计算位移
        x_ += (vx * cos(th_) - vy * sin(th_)) * dt;
        y_ += (vx * sin(th_) + vy * cos(th_)) * dt;
        th_ += w * dt;

        // 设置odom消息
        message.header.stamp = current_time_;
        message.header.frame_id = "odom";
        message.child_frame_id = "base_link";

        message.pose.pose.position.x = x_;
        message.pose.pose.position.y = y_;
        message.pose.pose.position.z = 0.0;
        
        tf2::Quaternion q;
        q.setRPY(0, 0, th_);
        message.pose.pose.orientation = tf2::toMsg(q);

        // 发布odom消息
        odom_publisher_->publish(message);
        last_time_ = current_time_;
    }

    void get_current_pose()
    {
        try {
            // 获取从 map 到 base_link 的变换
            geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform(
                "map", "base_link", tf2::TimePointZero);
            
            // 提取位置和姿态
            double x = transform.transform.translation.x;
            double y = transform.transform.translation.y;
            double z = transform.transform.translation.z;
            double qx = transform.transform.rotation.x;
            double qy = transform.transform.rotation.y;
            double qz = transform.transform.rotation.z;
            double qw = transform.transform.rotation.w;

            RCLCPP_INFO(this->get_logger(), "Current pose: x=%.2f, y=%.2f, z=%.2f", x, y, z);
            RCLCPP_INFO(this->get_logger(), "Current orientation: qx=%.2f, qy=%.2f, qz=%.2f, qw=%.2f", qx, qy, qz, qw);
        } catch (tf2::TransformException &ex) {
            RCLCPP_WARN(this->get_logger(), "Could not get transform: %s", ex.what());
        }
    }
    
    // rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Time current_time_, last_time_;
    double x_{0.0}, y_{0.0}, th_{0.0};
    // size_t count_;

    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher_;
    rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr motor_control_;
    //添加订阅topic，获取目标点坐标
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr topic_sub_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr motor_info_;
    Motor_Circle *mc;
    yaogan *yg;
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
    float linex,liney,wmga;
    int yaml_run_mode,yaml_angle_n,yaml_angle_set;
};

int main(int argc, char ** argv)
{
  // rclcpp::InitOptions init_option;
	// init_option.set_domain_id(1);
	//rclcpp::init(argc, argv,init_option);
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<Motor_Driver>());
  rclcpp::shutdown();
  return 0;
}
